//
// Created by linfe on 2023/6/9.
//
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include "inscube_receiver.h"


/* This example creates a subclass of Node and uses std::bind() to register a
* member function as a callback from the timer. */
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr InsCubePublisher::publisher_;
size_t InsCubePublisher::count_ = 0;

InsCubePublisher::InsCubePublisher() : Node("b343") {
    qRegisterMetaType<LiveData>("LiveData");
    qRegisterMetaType<QSerialPort::SerialPortError>("QSerialPort::SerialPortError");
    publisher_ = this->create_publisher<std_msgs::msg::String>("demo", 10);
    auto serials = QSerialPortInfo::availablePorts();
    if (serials.isEmpty()) {
        LOG(ERROR) << "no serial is connect to this device";
        return;
    }
    auto info = serials[0];
    LOG(INFO) << "try to open device " << info.portName().toStdString();
    handler.SetInfo(info);
    handler.SetBaudRate((QSerialPort::BaudRate) 921600);
    th_serial = new QThread();
    handler.moveToThread(th_serial);
    QObject::connect(th_serial, &QThread::started, &handler, &SerialPortHandler::Init);
    QObject::connect(th_serial, &QThread::finished, &handler, &SerialPortHandler::Stop);
    std::function<void(uint32_t, uint8_t *)> callback = InsCubePublisher::OnCommSyncMsg;
    handler.SetCallBackFunc(this, comm_sync_msg_imu_id, callback);

//        QObject::connect(&handler, &SerialPortHandler::SerialStateChanged, this, &LiveViewWindow::on_SerialStateChanged);
    th_serial->start();

}

InsCubePublisher::~InsCubePublisher() {
    if (th_serial->isRunning()) {
        th_serial->exit(0);
    }
    delete th_serial;
}

 void InsCubePublisher::OnCommSyncMsg(uint32_t msg_id, uint8_t *msg_data) {
    LOG_EVERY_N(INFO, 1000) << __FUNCTION__;
    auto message = std_msgs::msg::String();
    message.data = "Hello, world! " + std::to_string(count_++);
    publisher_->publish(message);
}


#include "QCoreApplication"

int main(int argc, char *argv[]) {
    google::InitGoogleLogging(argv[0]);
    google::LogToStderr();
    QCoreApplication a(argc, argv);
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<InsCubePublisher>());
    a.exec();
    rclcpp::shutdown();
    return 0;
}